#include "../ui/ui_mainwindow.h"
#include "mainwindow.h"
#include "qdebug.h"
#include "qtimer.h"
#include "../user/udp_mavlink.h"
#include "../Mavlink_V1.0/all/mavlink.h"
#include "../user/gamepadthread.h"
#include <QDateTime>
#include "maincharts.h"



MainCharts * UserDataGraph;

//主窗口构造函数
MainWindow::MainWindow(QWidget *parent) : QMainWindow(parent) , ui(new Ui::MainWindow)
{
    ui->setupUi(this);

    ui->pm1_show->setDecMode(); //设置为10进制模式;
    ui->pm2_show->setDecMode();

    ui->udp_add->setText(User_Send->udp_add);
    ui->udp_port->setText(User_Send->udp_port_remote);
    ui->udp_localport->setText(User_Send->udp_port_local);

    work_status = 0;

    //控制逻辑线程
    QTimer *work_time = new QTimer(this);
    connect(work_time, SIGNAL(timeout()), this, SLOT(logic())); // slotCountMessage是我们需要执行的响应函数
    work_time->start(1); // 每隔1s

    //ui刷新线程
    QTimer *uirefresh = new QTimer(this);
    connect(uirefresh,SIGNAL(timeout()),this , SLOT (ui_refsh()));
    uirefresh->start(10);

    //mavlink常规消息发送线程
    QTimer *send = new QTimer(this);
    connect(send,SIGNAL(timeout()),this , SLOT (send_data()));
    send->start(100);



    //创建信号链接
    if (!connect(User_Send, User_Send->message_share, this,this->message_plain_set))
    {
        qDebug() << "Failed to connect message_share signal to message_plain_set slot";
    }
    else
    {
        qDebug() << "Successfully connected message_share signal to message_plain_set slot";
    }

    rc_channel[2] = 1500;
    rc_channel[3] = 1500;

    rc_channel[6] = 1500;
    rc_channel[7] = 1500;

    mode_status[0] = true;
    mode_status[1] = false;
    mode_status[2] = false;
    mode_status[3] = false;
    gamepad_ok = true;
    set_button_color();

}

MainWindow::~MainWindow()
{
    delete ui;
}

/**
 * 主窗口业务逻辑循环调用
 */
void MainWindow::logic()
{

    if (GamepadState.buttons_last.Start == 0 && GamepadState.buttons.Start == 1)
    {
        on_un_lock_stateChanged(true);
    }
    else if (GamepadState.buttons_last.Back == 0 && GamepadState.buttons.Back == 1)
    {
        on_un_lock_stateChanged(false);
    }


    if (GamepadState.buttons_last.X == 0 && GamepadState.buttons.X == 1) {

        on_wall_clicked(false);
    }
    if (GamepadState.buttons_last.Y == 0 && GamepadState.buttons.Y == 1) {

        on_fool_clicked(false);
    }

    if (GamepadState.buttons_last.A == 0 && GamepadState.buttons.A == 1) {

         on_cam_clicked(false);
    }
    if (GamepadState.buttons_last.B == 0 && GamepadState.buttons.B == 1) {
        on_rc_clicked(false);
    }

    if (GamepadState.buttons_last.DPadUp == 0 && GamepadState.buttons.DPadUp == 1) {
        rc_channel[6]+=20;
        if (rc_channel[6]>1900) {
            rc_channel[6]=1900;
        }
        ui->pm1->setValue(rc_channel[6]);

        rc_channel[7] +=20;
        if (rc_channel[7]>1900) {
            rc_channel[7]=1900;
        }
        ui->pm2->setValue(rc_channel[7]);
        GamepadState.buttons_last.DPadUp = true;


    }

    if (GamepadState.buttons_last.DPadDown == 0 && GamepadState.buttons.DPadDown == 1) {

        rc_channel[6]-=20;
        if (rc_channel[6]<1500) {
            rc_channel[6]=1500;
        }
        ui->pm1->setValue(rc_channel[6]);

        rc_channel[7] -=20;
        if (rc_channel[7]<1500) {
            rc_channel[7]=1500;
        }
        ui->pm2->setValue(rc_channel[7]);
        GamepadState.buttons_last.DPadDown = true;

    }

    if (GamepadState.buttons_last.LeftStick == 0 && GamepadState.buttons.LeftStick == 1) {
        ui->pm1->setValue(1500);
        ui->pm2->setValue(1500);
        GamepadState.buttons_last.LeftStick = true;
    }


}


void MainWindow::ui_refsh() {
    /*ui 显示手柄的遥控信息 同步遥控信息到控件内 控件本身也是信息输入源*/
    if(gamepad_ok)
    {
        ui->fb->setValue(1500 + (GamepadState.leftAxis.y * 400));
        ui->lr->setValue(1500 + (GamepadState.leftAxis.x * 400));

        // ui->pm1->setValue(1500 + (GamepadState.leftTrigger * 400)); //不再使用扳机控制泵速
        // ui->pm2->setValue(1500 + (GamepadState.rightTrigger * 400));
    }

    /*控件归位*/
    if(work_status == 0) //如果是锁定状态就给遥感赋1500
    {
        ui->fb->setValue(1500);
        ui->lr->setValue(1500);
        ui->pm1->setValue(1500);
        ui->pm2->setValue(1500);

        ui->fb_show->display(QString::number(1500));
        ui->lr_show->display(QString::number(1500));
        ui->pm1_show->display(QString::number(1500));
        ui->pm2_show->display(QString::number(1500));
    }
    //显示
    ui->fw_version->setText(User_Send->robo_info.fw_version);//固件版本
    ui->hw_version->setText(User_Send->robo_info.hw_version);//硬件版本
    ui->bt_version->setText(User_Send->robo_info.bt_version);//蓝牙版本

    ui->clean_mode->setText(User_Send->robo_info.clean_mode);//清洁模式
    ui->now_step->setText(User_Send->robo_info.now_state);//当前步数

    ui->battery_volt->setText(User_Send->robo_info.battery_volt);//电池电压 V
    ui->battery_percent->setText(User_Send->robo_info.battery_percent);//电池百分比 %
    ui->battery_crrut->setText(User_Send->robo_info.battery_current);//电池电流 mA
    ui->battery_power->setText(User_Send->robo_info.battery_power); //功率 W

    ui->has_water_in->setText(User_Send->robo_info.has_water_in); //机舱有水
    ui->has_water_left->setText(User_Send->robo_info.has_water_left);//左遇水传感器
    ui->has_water_right->setText(User_Send->robo_info.has_water_right);//右遇水传感器

    ui->moto_rmp_left->setText(QString::number(User_Send->robo_info.moto_rmp_left));//左电机
    ui->moto_rmp_right->setText(QString::number(User_Send->robo_info.moto_rmp_right));//右电机转速
    ui->pump_rmp_left->setText(QString::number(User_Send->robo_info.pump_rmp_left));//左泵
    ui->pump_rmp_right->setText(QString::number(User_Send->robo_info.pump_rmp_right));//右泵

    ui->flow_x->setText(QString::number(User_Send->robo_info.flow_x));//水流x
    ui->flow_y->setText(QString::number(User_Send->robo_info.flow_y));//水流y
    ui->flow_x_int->setText(QString::number(User_Send->robo_info.flow_x_sum));//水流x累加
    ui->flow_y_int->setText(QString::number(User_Send->robo_info.flow_y_sum));//水流y累加

    ui->lidar->setText(QString::number(User_Send->robo_info.lidar[0])+","+
        QString::number(User_Send->robo_info.lidar[1])+","+
        QString::number(User_Send->robo_info.lidar[2])+","+
        QString::number(User_Send->robo_info.lidar[3]));

    //将毫秒转化为 时:分:秒
    uint32_t totalSeconds = User_Send->robo_info.time_tick / 1000;
    uint32_t hours = totalSeconds / 3600;
    uint32_t minutes = (totalSeconds % 3600) / 60;
    uint32_t seconds = totalSeconds % 60;
    QString timeString = QString("%1:%2:%3")
        .arg(hours, 2, 10, QChar('0'))
        .arg(minutes, 2, 10, QChar('0'))
        .arg(seconds, 2, 10, QChar('0'));
    ui->time_tick->setText(timeString); // 时间戳
    ui->how_high->setText(QString::number(User_Send->robo_info.high_in,'f',3));
    ui->roll->setText(QString::number(User_Send->robo_info.euler.x*180/3.1415962,'f',6));
    ui->pitch->setText(QString::number(User_Send->robo_info.euler.y*180/3.1415962,'f',6));
    ui->yaw->setText(QString::number(User_Send->robo_info.euler.z*180/3.1415962,'f',6));

    ui->quat_w->setText(QString::number(User_Send->robo_info.quat[0],'f',6));
    ui->quat_x->setText(QString::number(User_Send->robo_info.quat[1],'f',6));
    ui->quat_y->setText(QString::number(User_Send->robo_info.quat[2],'f',6));
    ui->quat_z->setText(QString::number(User_Send->robo_info.quat[3],'f',6));

    // float *imu_q = User_Send->robo_info.quat;
    // float cnb[3][3] = {
    //     {(imu_q[0] * imu_q[0] + imu_q[1] * imu_q[1] - imu_q[2] * imu_q[2] - imu_q[3] * imu_q[3]), 2 * (imu_q[1] * imu_q[2] + imu_q[0] * imu_q[3]), 2 * (imu_q[1] * imu_q[3] - imu_q[0] * imu_q[2])},
    //     {2 * (imu_q[1] * imu_q[2] - imu_q[0] * imu_q[3]), imu_q[0] * imu_q[0] - imu_q[1] * imu_q[1] + imu_q[2] * imu_q[2] - imu_q[3] * imu_q[3], 2 * (imu_q[2] * imu_q[3] + imu_q[0] * imu_q[1])},
    //     {2 * (imu_q[1] * imu_q[3] + imu_q[0] * imu_q[2]), 2 * (imu_q[2] * imu_q[3] - imu_q[0] * imu_q[1]), imu_q[0] * imu_q[0] - imu_q[1] * imu_q[1] - imu_q[2] * imu_q[2] + imu_q[3] * imu_q[3]}};
    //
    // ui->vec_xx->setText(QString::number(cnb[0][0],'f',6));
    // ui->vec_yx->setText(QString::number(cnb[0][1],'f',6));
    // ui->vec_zx->setText(QString::number(cnb[0][2],'f',6));
}

void MainWindow::send_data() {

    //如果是遥控模式 才发送遥控信息
    if(mode_status[3]==1)
    {
        uint16_t pm_o[2];
        float gg;
        if (abs(GamepadState.rightAxis.x) <0.5) {
            gg = 0;
        }else {
            gg= GamepadState.rightAxis.x;
        }

        pm_o[0] = rc_channel[6]+ (uint16_t)(-gg * 150) ;
        pm_o[1] = rc_channel[7]+(uint16_t)(gg * 150);
        if (pm_o[0]< 1500) {
            pm_o[0] = 1500;
        }
        if (pm_o[1]< 1500) {
            pm_o[1] = 1500;
        }
        if (pm_o[0]>1900) {
            pm_o[0] = 1900;
        }
        if (pm_o[1]>1900) {
            pm_o[1] = 1900;
        }
        mavlink_message_t  mav_buff;
        mavlink_msg_rc_channels_override_pack(255,25,&mav_buff,1,1,rc_channel[0],rc_channel[1],rc_channel[2],rc_channel[3],rc_channel[4],rc_channel[5],pm_o[0],pm_o[1]);
        uint8_t  send_data[255];
        char len =  mavlink_msg_to_send_buffer(send_data, &mav_buff);
        User_Send->mavlink_send_udp((char *)send_data,len);
    }


    uint16_t Rumble_lift =0;
    uint16_t Rumble_right =0;

    //将姿态按照高低频特性映射到手柄震动;
    Rumble_lift = (uint16_t) ((fabsf(User_Send->robo_info.gyro.x)/10000) * 0xffff);
    Rumble_right = (uint16_t) ((fabsf(User_Send->robo_info.acc.x-User_Send->robo_info.acc_last.x)/50000) * 0xffff);
    if (Rumble_lift > 0xffff)
    {
        Rumble_lift = 0xffff;
    }
    if (Rumble_right > 0xffff)
    {
        Rumble_right = 0xffff;
    }
    if (Rumble_lift < 100) {
        Rumble_lift = 0;
    }

    if (Rumble_right <150) {
        Rumble_right = 0;
    }
    UsergamepadThread->setRumble(Rumble_lift ,
        Rumble_right , 100);


}


void MainWindow::on_pm1_show_overflow()
{

}

void MainWindow::on_pm2_show_overflow()
{

}

void MainWindow::on_pm1_valueChanged(int value)
{
    ui->pm1_show->display(QString::number(value));
    rc_channel[6] = value;
}


void MainWindow::on_pm2_valueChanged(int value)
{
    ui->pm2_show->display(QString::number(value));
    rc_channel[7] = value;
}


void MainWindow::on_fb_valueChanged(int value)
{
    if(value>1495 && value < 1555)
    {
        ui->fb->setValue(1500);
        value = 1500;
    }
    ui->fb_show->display(QString::number(value));
    rc_channel[2] = value;// 三号通道 前后通道
}

void MainWindow::on_lr_valueChanged(int value)
{
    if(value>1495 && value < 1555)
    {
        ui->lr->setValue(1500);
        value = 1500;
    }
    ui->lr_show->display(QString::number(value));
    rc_channel[3] = value; // 4th channel is left&right channel
}


void MainWindow::on_udp_add_textEdited(const QString &arg1)
{
    User_Send->udp_add = arg1;
}


void MainWindow::on_udp_port_textEdited(const QString &arg1)
{
     User_Send->udp_port_remote = arg1;
}

void MainWindow::on_udp_localport_textEdited(const QString &arg1) {
    User_Send->udp_port_local = arg1;
    //先断开当前udp连接
    User_Send->server->close();
    if (!User_Send->server->bind(QHostAddress::Any, User_Send->udp_port_local.toUShort())) {
        qDebug() << "Bind Failed";
    } else {
        qDebug() << "Bind Success";

    }
    //重新连接udp
    User_Send->server->connectToHost(QHostAddress(User_Send->udp_add), User_Send->udp_port_remote.toInt());
    User_Send->server->waitForConnected();
    qDebug() << "连接成功";
}

void MainWindow::on_wall_clicked(bool checked)
{

    if(mode_status[0] == 0) //非当前工作模式
    {
        mode_status[0] = 1;
        work_status = 0;
    }
    mode_status[1] = false;
    mode_status[2] = false;
    mode_status[3] = false;
    set_button_color();
    set_mode();
}
void MainWindow::on_fool_clicked(bool checked)
{

    if(mode_status[1] == 0)
    {
        mode_status[1] = 1;
        work_status = 0;

    }
    mode_status[0] =  false;
    mode_status[2] =  false;
    mode_status[3] =  false;
    set_button_color();
    set_mode();
}
void MainWindow::on_cam_clicked(bool checked)
{

    if(mode_status[2] == 0)
    {
        mode_status[2] = 1;
        work_status = 0;

    }
    mode_status[0] =  false;
    mode_status[1] =  false;
    mode_status[3] =  false;
    set_button_color();
    set_mode();
}
void MainWindow::on_rc_clicked(bool checked)
{

    if(mode_status[3] == 0)
    {
        mode_status[3] = 1;
        work_status = 0;
    }
    mode_status[0] =  0;
    mode_status[1] =  0;
    mode_status[2] =  0;
    set_button_color();
    set_mode();
}


void MainWindow::set_button_color()
{
    QString GRREN = "QPushButton {"
                    "background-color: rgb(0, 160, 0);" // 背景色
                    "color: white;" // 文字颜色
                    "border-radius: 5px;" // 圆角边框
                    "border: 2px solid rgb(0,100,0);" // 边框样式
                    "}"
                    "QPushButton:hover {"
                    "background-color: rgb(0, 200, 0);" // 鼠标悬停时的背景色
                    "}"
                    "QPushButton:pressed {"
                    "background-color: rgb(0, 100, 0);" // 按下时的背景色
                    "}";
    QString RED = "QPushButton {"
                  "background-color: rgb(170, 0, 0);" // 背景色
                  "color: white;" // 文字颜色
                  "border-radius: 5px;" // 圆角边框
                  "border: 2px solid rgb(150, 0, 0);" // 边框样式
                  "}"
                  "QPushButton:hover {"
                  "background-color: rgb(200, 0, 0);" // 鼠标悬停时的背景色
                  "}"
                  "QPushButton:pressed {"
                  "background-color: rgb(150, 0, 0);" // 按下时的背景色
                  "}";

    if(mode_status[0])
    {
        ui->wall->setStyleSheet(GRREN);
    }
    else
    {
        ui->wall->setStyleSheet(RED);
    }

    if(mode_status[1])
    {
        ui->fool->setStyleSheet(GRREN);
    }
    else
    {
        ui->fool->setStyleSheet(RED);
    }

    if(mode_status[2])
    {
        ui->cam->setStyleSheet(GRREN);
    }
    else
    {
        ui->cam->setStyleSheet(RED);
    }

    if(mode_status[3])
    {
        ui->rc->setStyleSheet(GRREN);
    }
    else
    {
        ui->rc->setStyleSheet(RED);
    }

}

/**
 * @brief 设置模式
 */
void MainWindow::set_mode()
{
    mavlink_message_t  mav_buff;
    uint8_t  send_data[255];
    char len;

    char mode_set;
    if(mode_status[0]) // 池壁模式
    {
        mode_set = wall_mode;
    }
    else if(mode_status[1]) //池底
    {
        mode_set = fool_mode;
    }
    else if(mode_status[2]) //相机模式
    {
        mode_set = carmera;
    }
    else if(mode_status[3])//设置为遥控模式
    {
        mode_set = rc_ctrl;
    }
    on_un_lock_stateChanged(false);
    //send wash mode command
    mavlink_msg_command_long_pack(255,25,&mav_buff,1,1,MAV_CMD_DO_SET_MODE,0,MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,mode_set,0,0,0,0,0);
    len =  mavlink_msg_to_send_buffer(send_data, &mav_buff);
    User_Send->mavlink_send_udp((char *)send_data,len);
}
/**
 * @brief 设置显示的数据 将其地址填入列表中
 * @note 被显示ui按钮调用
 */
void MainWindow::set_data_list() {

    //检查勾选的需要显示的数据 并得到数量

    uint8_t data_num = 0;

    //设置显示的数据
    for (int i = 0; i < data_num; i++) {
        data_list[i] = &User_Send->robo_info.high_in;
    }



}


/************* ui 槽函数 *************/
/**
 *
 * @param arg1
 */
void MainWindow::on_un_lock_stateChanged(int arg1)
{
    //解锁命令发送
 if (arg1 == 0)
    {
        work_status = false;
        ui->un_lock->setChecked(false); //确保非ui调用时,也能刷新ui
    }
    else {
        work_status = true;
        ui->un_lock->setChecked(true); //确保非ui调用时,也能刷新ui
    }
    mavlink_message_t  mav_buff;
    uint8_t  send_data[255];
    char len;
    //send lock or unlock command
    mavlink_msg_command_long_pack(255,25,&mav_buff,1,1,MAV_CMD_COMPONENT_ARM_DISARM,0,work_status,0,0,0,0,0,0);
    len =  mavlink_msg_to_send_buffer(send_data, &mav_buff);
    User_Send->mavlink_send_udp((char *)send_data,len);


}

void MainWindow::on_gamepad_ctrl_stateChanged(int arg1)
{
    if(arg1!=0)
    {
        gamepad_ok = true;
    }
    else
    {
        gamepad_ok = false;
    }

}
/* 自定义文本消息接收函数 */
 void MainWindow::message_plain_set(const QByteArray *data, uint8_t len) const {
    //获取系统时间
    QDateTime current_time = QDateTime::currentDateTime();
    QString time_str = current_time.toString("[hh:mm:ss]:");

    if (ui->mesage_rtime->isChecked() == true) {
        // 如果不包含"waring"关键字，则使用默认格式显示消息
        ui->mesage_plain->appendPlainText(time_str + QString(data->data()));
        // 将光标移动到文本末尾
        ui->mesage_plain->moveCursor(QTextCursor::End);
    }

}

void MainWindow::on_clean_plain_clicked() const {
    ui->mesage_plain->clear();
}

void MainWindow::on_clean_flow_int_clicked() {
    User_Send->robo_info.flow_x_sum = 0;
    User_Send->robo_info.flow_y_sum = 0;
}

/**
 * @brief 打开图表界面
 */
void MainWindow::on_open_charts_clicked() {
    //打开图表界面
    // 绘图实例 如果mainwindow已经存在，则直接显示，否则新建一个实例
    if ( UserDataGraph == nullptr )
        {
        // ui->check_high.isChecked() = true;
        UserDataGraph = new MainCharts(&User_Send->robo_info.high_in,1);
        UserDataGraph->setWindowTitle("数据窗");
        UserDataGraph->show();
    }
    else {
        //已经存在实例 所以把这个实例顶置
        UserDataGraph->show();
        UserDataGraph->raise();
    }
}



